#include "btl_config.h"
#include "fp_task.h"
#include "fp_algorithm.h"
#include "string.h"
#include "fp_interrupt_reset.h"
#include "fp_util.h"
#include "fp_timer.h"
#include "protocol.h"
#include "fingerprint.h"

#define BTL_MATH_MAX(a, b) ((a) > (b) ? (a) : (b))

static int do_detected_cnt = 0;

fp_task_t __fp_task_block;
fp_task_t* fp_task(void)
{
    return &__fp_task_block;
}

void task_run(uint8_t task_id, void (*run)(void))
{
    fp_task_t* task = fp_task();
    task->run = run;
    task->task_id = task_id;
    task->task_begin = fp_timer_get();
}

void task_next(void)
{
    fp_task_t* task = fp_task();
    if (task->task_id == TASK_ID_TFD)
        task_run(TASK_ID_IDLE, do_idle);
}

void task_default(void)
{
    // fp_task_t* task = fp_task();
    unsigned int nr = btl_storage_get_enroll_template_count();

    bf_print_d("fingerprint count: %u", nr);
    if (nr > 0)
        fp_do_verify();
    else
        fp_do_detect1();

    /* Release task lock */
    taskrun_unlock();
}

int is_task_timeout(void)
{
    fp_task_t* task = fp_task();
    unsigned int now = fp_timer_get();
    unsigned int diff = now - task->task_begin;
    if (diff > task->timeout)
        return 1;
    else
        return 0;
}

void update_task_timeout(void)
{
    fp_task_t* task = fp_task();
    task->task_begin = fp_timer_get();
}

void task_tfd(void)
{
    fp_task_t* task = fp_task();
    task->task_id   = TASK_ID_TFD;
}

void fp_do_idle(void)
{
    fp_task_t* task = fp_task();
    SET_FP_TASK(task, task_ev, FP_TASK_EV_OTHER);
    btl_set_work_mode(BTL_IDLE_MODE);

    task_run(TASK_ID_IDLE, do_idle);
}

void fp_do_detect(void)
{
    // bf_print_d("enter");
    fp_task_t* task = fp_task();
    SET_FP_TASK(task, task_ev, FP_TASK_EV_OTHER);

    do_detected_cnt = 0;
    btl_set_work_mode(BTL_FINGER_DETECT_MODE);
    task_run(TASK_ID_WFD, do_detect);
}

void fp_do_detect1(void)
{
    // bf_print_d("enter");
    fp_task_t* task = fp_task();
    btl_get_config()->image_quality_for_verify.min_quality = 30;
    btl_set_work_mode(BTL_VERIFY_MODE);
    task_run(TASK_ID_WFD, do_detect1);
}

#define OVERLAP_THRESHOLD 3
static int much_overlap_cnt = 0;
void fp_do_enroll(void)
{
    fp_task_t *task = fp_task();
    WORK_STRUCTURE* work = fp_work_info();
    SET_FP_TASK(task, task_ev, FP_TASK_EV_ENROLL);

    much_overlap_cnt = 0;
    TEnrollOverlapParam enroll_overlap_param = btl_get_enroll_overlap_param();
    enroll_overlap_param.duplicate_min_distance = DUPLICATE_MIN_DISTANCE;  // 30
    enroll_overlap_param.nbr_to_check_redundancy_against = MAX_CHECK_REDUNDANCY_AGAINST;

    btl_set_enroll_overlap_param(&enroll_overlap_param);

    btl_ai_delete_all_subtemplate();
    int retval = btl_set_work_mode(BTL_ENROLLMENT_MODE);
    bf_print_d("set work mode: BTL_ENROLLMENT_MODE, retval = %d", retval);
    task->timeout = DO_ENROLL_TIMEOUT;
    work->AcceptedNum = 0;

    task_run(TASK_ID_WFD, do_enroll);
}

void fp_do_checkfingeroff(void)
{
    // bf_print_d("enter");
    fp_task_t* task = fp_task();
    SET_FP_TASK(task, task_ev, FP_TASK_EV_OTHER);
    btl_set_work_mode(BTL_FINGER_DETECT_MODE);
    task_run(TASK_ID_WFD, do_checkfingeroff);
}

void fp_do_verify(void)
{
    fp_task_t* task = fp_task();
    btl_get_config()->image_quality_for_verify.min_quality = 20;
    int retval = btl_set_work_mode(BTL_VERIFY_MODE);
    bf_print_d("II:%d", retval);
    SET_FP_TASK(task, task_ev, FP_TASK_EV_VERIFY);

    task_run(TASK_ID_WFD, do_verify);
}

void fp_do_Identifyremove(void)
{
    fp_task_t* task = fp_task();
    int retval = btl_set_work_mode(BTL_VERIFY_MODE);
    // DEBUG("II:%d", retval);
    // SET_FP_TASK(task, task_ev, FP_TASK_EV_VERIFY);
    bf_print_d("enter");
    task->timeout = DO_REMOVE_TIMEOUT;
    task_run(TASK_ID_WFD, do_Identifyremove);
}

void do_enroll(void)
{
    WORK_STRUCTURE* work = fp_work_info();
    FP_OPT* opt          = fp_option_info();
    fp_task_t* task      = fp_task();

    if (is_task_timeout()) {
        bf_print_d("timeout");
        notify(TASK_EXITVIP_LED, 0, 0);
        btl_ai_delete_all_subtemplate();
        fp_do_idle();
        return;
    }

    uint8_t data[2] = {0xFF};

    switch (task->task_fd) {
        case BTL_RC_OK:
            data[0] = FP_OK;
            break;
        // case BTL_RC_FINGER_DETECT_ERR:
        //     data[0] = FP_NOT_FINGER_DETECT;
        //     break;
        case BTL_RC_IMAGE_AREA_ERR:
            data[0] = FP_PARTIAL;
            break;
        case BTL_RC_IMAGE_NG:
        case BTL_RC_IMAGE_DRY_ERR:
        case BTL_RC_IMAGE_HUMIDITY_ERR:
        case BTL_RC_STATIC_PATTERN_ERR:
        case BTL_RC_CRACK_ERR:
            data[0] = FP_BAD_QUALITY;
            break;
        default:
            /* Not finger down info */
            if (task->task_fd != BTL_RC_FINGER_DETECT_ERR) {
                bf_print_d("finger detect: 0x%02x", task->task_fd);
            }
            return;
    }

    if (work->AcceptedNum == 0)
        notify(TASK_RESET_LED, 0, 0);

    update_task_timeout();
    bf_print_d("image: r = %x, spend %d ms", data[0], task->task_time);
    /* Finger down detected */
    // task_tfd();
#define DUPLICATE_FINGER_REJECT 1
    int retval = -1;

    if (data[0] == FP_OK) {
        do {
            /* Duplicate finger check */
            if (DUPLICATE_FINGER_REJECT) {
                // btl_get_config()->latency_adjustment = -100;
                int matchid = -1;
                retval = btl_ai_verify(&matchid);
                if (retval == BTL_RC_OK && matchid >= 0) {
                    data[0] = FP_DUPLICATE;
                    bf_print_d("duplicate fingerprint id: %d", matchid);
                    break;
                }
            }

            retval = btl_ai_add_subtemplate(&(work->AcceptedNum));
            switch (retval) {
                case BTL_RC_MUCH_OVERLAP: {
                    data[0] = FP_OVERLAP;
                    ++much_overlap_cnt;
                    
                    TEnrollOverlapParam enroll_overlap_param = btl_get_enroll_overlap_param();

                    if (much_overlap_cnt >= OVERLAP_THRESHOLD)
                        enroll_overlap_param.duplicate_min_distance = 0;
                    else
                        enroll_overlap_param.duplicate_min_distance =
                            BTL_MATH_MAX(0, DUPLICATE_MIN_DISTANCE *
                                                (MAX_CHECK_REDUNDANCY_AGAINST - work->AcceptedNum) /
                                                MAX_CHECK_REDUNDANCY_AGAINST);

                    btl_set_enroll_overlap_param(&enroll_overlap_param);
                }
                    break;
                case BTL_RC_EXIST_ENROLL_ID:
                    data[0] = FP_DUPLICATE;
                default:
                    break;
            }

        } while (0);
    }

    bf_print_d("enroll: accepted %d, r = 0x%x", work->AcceptedNum, retval);

    if (work->AcceptedNum < opt->MaxEnrollTimes) {
        if (data[0] == FP_OK)
            notify(TASK_ENROLL_LED, TASK_ENROLL_ADDED_LED, 1);
        else
            notify(TASK_ENROLL_LED, TASK_ENROLL_ADDED_LED, 0);
    }

    // DEBUG(" BTL-RC: 0x%x - error-code: 0x%x, accept: %d", task->task_fd, data[0], data[1]);
    // fp_send_command(FP_CMD0_DO_ENROLL, FP_CMD1_ENROLL, data, sizeof(data));
    if (work->AcceptedNum >= opt->MaxEnrollTimes) {
        unsigned short template_id;
        btl_storage_get_new_id_for_enroll_template(&template_id);
        btl_rc_t r = btl_ai_create_mutiltemplate(template_id, NULL);
        if (r == 0) {
            notify(TASK_ENROLL_LED, TASK_ENROLL_COMPLETED_LED, 1);
            /* Task completed */
            bf_print_d("enroll: completed: id = %d", template_id);
            fp_do_idle();
        } else {
            notify(TASK_ENROLL_LED, TASK_ENROLL_ADDED_LED, 0);
        }
    }
}

void do_verify(void)
{
    fp_task_t* task = fp_task();
    WORK_STRUCTURE* work = fp_work_info();

    int retval = -1;

    unsigned char feedback = FP_GENERAL_ERROR;
    switch (task->task_fd) {
        case BTL_RC_OK:
            feedback = FP_OK;
            break;
        // case BTL_RC_FINGER_DETECT_ERR:
        //     data[0] = FP_NOT_FINGER_DETECT;
        //     break;
        case BTL_RC_IMAGE_AREA_ERR:
            feedback = FP_PARTIAL;
            break;
        case BTL_RC_IMAGE_NG:
        case BTL_RC_IMAGE_DRY_ERR:
        case BTL_RC_IMAGE_HUMIDITY_ERR:
        case BTL_RC_STATIC_PATTERN_ERR:
        case BTL_RC_CRACK_ERR:
            feedback = FP_BAD_QUALITY;
            break;
        default:
            /* Not finger down info */
            if (task->task_fd != BTL_RC_FINGER_DETECT_ERR) {
                bf_print_d("finger detect: 0x%02x", task->task_fd);
            }
            return;
    }

    taskrun_lock();

    /* Finger down detected */
    // task_tfd();
    bf_print_d("image: spend %d ms", task->task_time);
    bf_print_d("IID:%d,%d", task->task_fd, feedback);
    memset(work, 0, sizeof(WORK_STRUCTURE));
    int matched_id = -1;

    if (feedback == FP_OK) {
        // TIME_MEASURE_INIT(extract);
        // btl_extract_template(NULL);
        // TIME_MEASURE_DIFF_PRINT(extract, "extract");
        btl_get_config()->latency_adjustment = - btl_storage_get_all_template_count() * 5 / 2;
        TIME_MEASURE_INIT(identify);
        btl_ai_verify(&matched_id);
        TIME_MEASURE_DIFF_PRINT(identify, "identify");
        bf_print_d("match:%d", matched_id);
    }

    if (matched_id >= 0)
        notify(TASK_VERIFY_LED, TASK_VERIFY_COMPLETED_LED, 1);
    else
        notify(TASK_VERIFY_LED, TASK_VERIFY_COMPLETED_LED, 0);

#if 0
    TIME_MEASURE_INIT(update);
    TIME_MEASURE_UPDATE(update);
    /* update template */
    if (matched_id >= 0) {
        retval = btl_ai_update(matched_id, BSUT_UPDATE_TEMPLATE | BSUT_UPDATE_OTHER_INFO);
        bf_print_d("IID update:%d", retval);
    }
    TIME_MEASURE_DIFF_PRINT(update, "update");
#endif

   measure_lock(matched_id);
}

void do_Identifyremove(void)
{
    fp_task_t* task      = fp_task();
    int retval = -1;
    unsigned char feedback = 0;

    if (is_task_timeout()) {
        bf_print_d("timeout!!!");
        notify(TASK_EXITVIP_LED, 0, 0);
        fp_do_idle();
        return;
    }

    switch (task->task_fd) {
        case BTL_RC_OK:
            feedback = FP_OK;
            break;
        // case BTL_RC_FINGER_DETECT_ERR:
        //     data[0] = FP_NOT_FINGER_DETECT;
        //     break;
        case BTL_RC_IMAGE_AREA_ERR:
            feedback = FP_PARTIAL;
            break;
        case BTL_RC_IMAGE_NG:
        case BTL_RC_IMAGE_DRY_ERR:
        case BTL_RC_IMAGE_HUMIDITY_ERR:
        case BTL_RC_STATIC_PATTERN_ERR:
        case BTL_RC_CRACK_ERR:
            feedback = FP_BAD_QUALITY;
            break;
        default:
            /* Not finger down info */
            return;
    }

    // /* Finger down detected */
    // task_tfd();

    bf_print_d("IID:%d,%d", task->task_fd, feedback);
    int matched_id = -1;
    int r = -1;

    if (feedback == FP_OK) {
        btl_ai_verify(&matched_id);
        bf_print_d("match:%d", matched_id);
        if (matched_id == 0 || matched_id == 1) {
            r = btl_storage_remove_all_template();
        } else if (matched_id > 1) {
            r = btl_storage_remove_template(matched_id);
        }
    } else {
        feedback = FP_NOT_MATCH;
    }

    if (r == 0) {
        task_tfd();
        notify(TASK_REMOVE_LED, TASK_REMOVE_COMPLETED_LED, 1);
    } else {
        notify(TASK_REMOVE_LED, TASK_REMOVE_COMPLETED_LED, 0);
    }
}

void do_detect(void)
{
    struct fp_protocol_info* info = fp_info();
    fp_task_t* task = fp_task();
    // uint8_t data    = 0;

    switch (task->task_fd) {
        case BTL_RC_OK: {
            if (info->recovery_mode) {
                bf_print_d("finger on");
                if (!info->fingerdown)
                    info->fingerdown = 1;
                info->recovery_id[0] = info->recovery_id[1]; /* 前一次按压时间 */
                info->recovery_id[1] = fp_timer_get();       /* 当前按压时间 */
                info->recovery_id_cnt++;
                // notify(TASK_RECOVERY_LED, TASK_RECOVERY_CHECK_LED, 0);
                notify(TASK_RECOVERY_LED, TASK_RECOVERY_INPUT_LED, 0);
                fp_do_checkfingeroff();
            } else {
                /* 延后等待手指离开后上报比对结果 */
                do_detected_cnt++;
                if (do_detected_cnt > 2) {
                    bf_print_d("finger on");
                    notify(TASK_VERIFY_LED, TASK_VERIFY_COMPLETED_LED, 1);
                    measure_lock(0 /*matched_id*/);
                }
            }

        } break;
        case BTL_RC_FINGER_DETECT_ERR:
        default: {
            // data = 0; /* 手指不存在 */
            // fp_send_command(FP_CMD0_FINGER_DETECT, 0, &data, sizeof(data));
            break;
        }
    }
}

void do_detect1(void)
{
    struct fp_protocol_info* info = fp_info();
    fp_task_t* task = fp_task();
    // uint8_t data    = 0;

    switch (task->task_fd) {
        case BTL_RC_OK: {
            taskrun_lock();
            bf_print_d("finger on");
            notify(TASK_VERIFY_LED, TASK_VERIFY_COMPLETED_LED, 1);
            measure_lock(0 /*matched_id*/);
        } break;
        case BTL_RC_FINGER_DETECT_ERR:
        default: {
            // data = 0; /* 手指不存在 */
            // fp_send_command(FP_CMD0_FINGER_DETECT, 0, &data, sizeof(data));
            break;
        }
    }
}

void sched_task_loop(void);
void do_checkfingeroff(void)
{
    fp_task_t* task = fp_task();
    struct fp_protocol_info* info = fp_info();
    // uint8_t data = 0;
    sched_task_loop();

    switch (task->task_fd) {
        case BTL_RC_OK: {
            // data = 1; /* 手指存在 */
            if (!info->fingerdown)
                info->fingerdown = 1;
            // bf_print_d("finger on");
            /* Nothing to do */
            // fp_send_command(FP_CMD0_FINGER_DETECT, 0, &data, sizeof(data));
        } break;
        case BTL_RC_FINGER_DETECT_ERR:
        default: {
            // data = 0; /* 手指不存在 */
            // fp_send_command(FP_CMD0_FINGER_DETECT, 0, &data, sizeof(data));

            if (info->fingerdown) {
                bf_print_d("finger off");
                info->fingerdown = 0;
            }

            if (info->recovery_mode) {
                info->recovery_lasttime = fp_timer_get();
                fp_do_detect();
            }
            break;
        }
    }

    // task_tfd();
    // fp_do_idle();
}

void do_idle(void)
{
    /* Nothing to do */
}
